package deformation.point;

import java.util.List;
import java.util.ListIterator;

import deformation.AbstractPointDeformation;
import deformation.PreCompute;
import grid.GridPoint;
import handler.DeformationPoint;
import handler.PointHandler;

public class RigidPointDeformation extends AbstractPointDeformation<RigidPointDeformation.PreData>{

	private final double alpha;
	
	public RigidPointDeformation(PointHandler pHandler) {
		super(pHandler);
		this.alpha=1;
	}
	
	public RigidPointDeformation(PointHandler pHandler, double alpha) {
		super(pHandler);
		this.alpha=alpha;
	}
	
	class PreData implements PreCompute{
		double[] A11;
		double[] A12;
		double[] A21;
		double[] A22;
		double[] weights;
		DeformationPoint pStar;
	}
	

	@Override
	public PreData preComputeGridPoint(GridPoint v) {
		//example how to access data
		List<DeformationPoint> basePoints = pointHandler.getBasePoints();
		//example ende
		PreData precomputedData=new PreData();
		
		// compute weights and pStar
		precomputedData.weights=new double[basePoints.size()];
		ListIterator<DeformationPoint> basePointIterator=basePoints.listIterator();
		int i=0;
		double sumWeight=0;
		double pStarX=0;
		double pStarY=0;
		while (basePointIterator.hasNext()){
			DeformationPoint basePoint=basePointIterator.next();
			precomputedData.weights[i]=1/Math.pow((basePoint.x-v.x)*(basePoint.x-v.x)+(basePoint.y-v.y)*(basePoint.y-v.y),alpha);
			pStarX+=precomputedData.weights[i]*basePoint.x;
			pStarY+=precomputedData.weights[i]*basePoint.y;
			sumWeight+=precomputedData.weights[i];
			i++;
		}
		DeformationPoint pStar = new DeformationPoint(pStarX/sumWeight, pStarY/sumWeight);
	
		//compute the Aj and mys (i.e. µ_s )
		precomputedData.A11 = new double[basePoints.size()];
		precomputedData.A12 = new double[basePoints.size()];
		precomputedData.A21 = new double[basePoints.size()];
		precomputedData.A22 = new double[basePoints.size()];
		for (int j=0; j<basePoints.size();j++){
			precomputedData.A11[j]=precomputedData.weights[j]*((basePoints.get(j).x-pStar.x)*(v.x-pStar.x)
					+ (basePoints.get(j).y-pStar.y)*(v.y-pStar.y));
			precomputedData.A12[j]=precomputedData.weights[j]*((basePoints.get(j).x-pStar.x)*(v.y-pStar.y)
					+ (basePoints.get(j).y-pStar.y)*(-v.x+pStar.x));
			precomputedData.A21[j]=precomputedData.weights[j]*((basePoints.get(j).y-pStar.y)*(v.x-pStar.x)
					+ (-basePoints.get(j).x+pStar.x)*(v.y-pStar.y));
			precomputedData.A22[j]=precomputedData.weights[j]*((basePoints.get(j).y-pStar.y)*(v.y-pStar.y)
					+ (-basePoints.get(j).x-pStar.x)*(-v.x+pStar.x));
		}
		
		precomputedData.pStar=pStar;
		return precomputedData;
	}
	
	
	/*
	 * computes and returns qStar=(sum w_*q_i)/(sum w_i)
	 */
	DeformationPoint getqStar(List<DeformationPoint> movedPoints, double[] weights){
		double weightsum=0;
		double qStarX=0;
		double qStarY=0;
		for (int i=0;i<movedPoints.size(); i++){
			qStarX+=weights[i]*movedPoints.get(i).x;
			qStarY+=weights[i]*movedPoints.get(i).y;
			weightsum+=weights[i];
		}
		return (new DeformationPoint(qStarX/weightsum,qStarY/weightsum));
	}
	
	
	@Override
	public GridPoint calculateGridPoint(GridPoint v, PreData precomputedData) {
		List<DeformationPoint> movedPoints = pointHandler.getMovedPoints(); //the moved handlers, i.e. the q's
		List<DeformationPoint> basePoints = pointHandler.getBasePoints(); 
		DeformationPoint qStar = getqStar(movedPoints, precomputedData.weights); // compute qStar 
		double fX=0; //x value for the returned GridPoint
		double fY=0; //y value for the returned GridPoint
		for (int i=0;i<movedPoints.size();i++){
			if ((basePoints.get(i).x==v.x) && (basePoints.get(i).y==v.y)) return (new GridPoint(movedPoints.get(i).x,movedPoints.get(i).y));
			fX+=((movedPoints.get(i).x-qStar.x)*precomputedData.A11[i]+(movedPoints.get(i).y-qStar.y)*precomputedData.A21[i]);
			fY+=((movedPoints.get(i).x-qStar.x)*precomputedData.A12[i]+(movedPoints.get(i).y-qStar.y)*precomputedData.A22[i]);
		}
		// copy variables for the normalization
		double copyX=fX;
		double copyY=fY;
		//divide the f vector by its norm and multiply with the norm of v-p
		fX=Math.sqrt((v.x-precomputedData.pStar.x)*(v.x-precomputedData.pStar.x)
				+((v.y-precomputedData.pStar.y)*(v.y-precomputedData.pStar.y)))
				*fX/(Math.sqrt(copyX*copyX+copyY*copyY));
		fY=Math.sqrt((v.x-precomputedData.pStar.x)*(v.x-precomputedData.pStar.x)
				+((v.y-precomputedData.pStar.y)*(v.y-precomputedData.pStar.y)))
				*fY/(Math.sqrt(copyX*copyX+copyY*copyY));
		
		//
		
		return (new GridPoint(fX+qStar.x, fY+qStar.y));
	}

}
